#define __RADAR_TASK_C_
#include "radar.h"
#include "gui_guider.h"
#include "events_init.h"
#include "custom.h"
#include <math.h>

TaskHandle_t radar_task_xhandle;
#define PORT_X 30
#define PORT_Y 26

typedef struct {
    double x;
    double y;
} Vector;

double vectorLength(Vector v) {
    return sqrt(v.x * v.x + v.y * v.y);
}

double dotProduct(Vector v1, Vector v2) {
    return v1.x * v2.x + v1.y * v2.y;
}

double angleBetweenVectors(Vector v1, Vector v2) {
    double cosTheta = dotProduct(v1, v2) / (vectorLength(v1) * vectorLength(v2));
    double theta = acos(cosTheta); // 得到的是弧度值
    double degrees = theta * (180.0 / M_PI); // 转换为度数
    return degrees;
}

void int_to_str(int num, char* str)
{
    // 处理负数
    if (num < 0) {
        *str++ = '-';
        num = -num;
    }

    // 求整数的位数
    int temp = num;
    int digits = 1;
    while (temp /= 10) {
        digits++;
    }

    // 逐位转换
    str += digits;
    *str = '\0';

    do {
        *--str = num % 10 + '0';
        num /= 10;
    } while (num);
}

char str_x_one[10], str_y_one[10];

typedef struct
{
    int32_t pos_x;
    int32_t pos_y;
    int32_t speed;
    int32_t resolution;
} radar_type_t;

typedef struct
{
    radar_type_t radar_one;
    radar_type_t radar_two;
    radar_type_t radar_three;
} radar_three_type_t;

extern xQueueHandle radar_queue;

extern void multiplayer_uart_send(void);
extern void single_uart_send(void);


void radar_task_process(void* msg)
{
    radar_three_type_t radar_value;
    int32_t one_x = 0;
    int32_t one_y = 0;
    int32_t two_x = 0;
    int32_t two_y = 0;
    int32_t three_x = 0;
    int32_t three_y = 0;
    int32_t one_x_temp = 0;
    int32_t one_y_temp = 0;
    uint8_t* p_test = (uint8_t*)malloc(4);
    Vector one_port;
    Vector two_port;
    Vector three_port;
    Vector fa_xian_port = { 0.0,800.0 };

    if (p_test == NULL) {
        printf("ptest is null11111!\r\n");
    }
    free(p_test);
    vTaskDelay(pdMS_TO_TICKS(1000));
    multiplayer_uart_send();
    int32_t angle_one = 0;
    int32_t angle_two = 0;
    int32_t angle_three = 0;
    while (1) {
        if (xQueueReceive(radar_queue, &radar_value, portMAX_DELAY)) {

            if (radar_value.radar_one.pos_x & 0x8000) {
                //50 为比例系数
                one_x = ((radar_value.radar_one.pos_x & 0x7FFF)+30) /  PORT_X;
                one_port.x = (double)((radar_value.radar_one.pos_x & 0x7FFF)+30);
            }
            else {
                one_x = (0 - ((radar_value.radar_one.pos_x & 0x7FFF)+30)) / PORT_X;
                one_port.x = (double)(0 - ((radar_value.radar_one.pos_x & 0x7FFF)+30));
            }

            if (radar_value.radar_one.pos_y & 0x8000) {
                one_y = (radar_value.radar_one.pos_y & 0x7FFF) / PORT_Y;
                one_port.y = (double)(radar_value.radar_one.pos_y & 0x7FFF);
            }
            else {
                one_y = (0 - (radar_value.radar_one.pos_y & 0x7FFF)) /PORT_Y;
                one_port.y = (double)(0 - (radar_value.radar_one.pos_y & 0x7FFF));
            }

            //计算夹角
            angle_one = (int32_t)radar_value.radar_one.pos_x & 0x8000? angleBetweenVectors(one_port, fa_xian_port):0-angleBetweenVectors(one_port, fa_xian_port);

            if (radar_value.radar_two.pos_x & 0x8000) {
                two_x = (radar_value.radar_two.pos_x & 0x7FFF) / PORT_X;
                two_port.x = (double)((radar_value.radar_two.pos_x & 0x7FFF)+30);
            }
            else {
                two_x = (0 - (radar_value.radar_two.pos_x & 0x7FFF)) / PORT_X;
                two_port.x = (double)(0 - ((radar_value.radar_two.pos_x & 0x7FFF)+30));
            }

            if (radar_value.radar_two.pos_y & 0x8000) {
                two_y = (radar_value.radar_two.pos_y & 0x7FFF) / PORT_Y;
                two_port.y = (double)(radar_value.radar_two.pos_y & 0x7FFF);
            }
            else {
                two_y = (0 - radar_value.radar_two.pos_y & 0x7FFF) / PORT_Y;
                two_port.y = (double)(0 - (radar_value.radar_two.pos_y & 0x7FFF));
            }
            angle_two = (int32_t)radar_value.radar_two.pos_x & 0x8000? angleBetweenVectors(two_port, fa_xian_port):0-angleBetweenVectors(two_port, fa_xian_port);

            if (radar_value.radar_three.pos_x & 0x8000) {
                three_x = (radar_value.radar_three.pos_x & 0x7FFF) / PORT_X;
                three_port.x = (double)((radar_value.radar_three.pos_x & 0x7FFF)+30);
            }
            else {
                three_x = (0 - (radar_value.radar_three.pos_x & 0x7FFF)) / PORT_X;
                three_port.x = (double)(0 - ((radar_value.radar_three.pos_x & 0x7FFF)+30));
            }
            if (radar_value.radar_three.pos_y & 0x8000) {
                three_y = (radar_value.radar_three.pos_y & 0x7FFF) / PORT_Y;
                three_port.y = (double)(radar_value.radar_three.pos_y & 0x7FFF);
            }
            else {
                three_y = (0 - (radar_value.radar_three.pos_y & 0x7FFF)) /PORT_Y;
                three_port.y = (double)(0 - (radar_value.radar_three.pos_y & 0x7FFF));
            }
            angle_three = (int32_t)radar_value.radar_three.pos_x & 0x8000? angleBetweenVectors(three_port, fa_xian_port):0-angleBetweenVectors(three_port, fa_xian_port);
            lv_obj_set_pos(guider_ui.screen_img_1, one_x+215, one_y+20);
            lv_label_set_text_fmt(guider_ui.screen_label_2, "Port1:%dmm %d°", (int)one_port.y, angle_one);

            lv_obj_set_pos(guider_ui.screen_img_2, two_x+215, two_y+20);
            lv_label_set_text_fmt(guider_ui.screen_label_3, "Port2:%dmm %d°", (int)two_port.y, angle_two);


            lv_obj_set_pos(guider_ui.screen_img_3, three_x+215, three_y+20);
            lv_label_set_text_fmt(guider_ui.screen_label_4, "Port3:%dmm %d°", (int)three_port.y, angle_three);

            if (one_x==0&&one_y==0&&radar_value.radar_three.speed==0) {
                lv_obj_add_flag(guider_ui.screen_img_1, LV_OBJ_FLAG_HIDDEN);
            }
            else {
                lv_obj_clear_flag(guider_ui.screen_img_1, LV_OBJ_FLAG_HIDDEN);
            }

            if (two_x==0&&two_y==0) {
                lv_obj_add_flag(guider_ui.screen_img_2, LV_OBJ_FLAG_HIDDEN);
            }
            else {
                lv_obj_clear_flag(guider_ui.screen_img_2, LV_OBJ_FLAG_HIDDEN);
            }

            if (three_x==0&&three_y==0) {
                lv_obj_add_flag(guider_ui.screen_img_3, LV_OBJ_FLAG_HIDDEN);
            }
            else {
                lv_obj_clear_flag(guider_ui.screen_img_3, LV_OBJ_FLAG_HIDDEN);
            }
            // printf("1 source :pos_x:%d \t pos_y:%d\r\n", radar_value.radar_one.pos_x, radar_value.radar_one.pos_y);
            // printf("1:pos_x:%d pos_y:%d speed:%d resolution:%d\r\n", one_x, one_y, radar_value.radar_one.speed, radar_value.radar_one.resolution);
            // printf("2:pos_x:%d pos_y:%d speed:%d resolution:%d\r\n", two_x, two_y, radar_value.radar_two.speed, radar_value.radar_two.resolution);
            // printf("3:pos_x:%d pos_y:%d speed:%d resolution:%d\r\n", three_x, three_y, radar_value.radar_three.speed, radar_value.radar_three.resolution);

        }
        vTaskDelay(pdMS_TO_TICKS(100));
    }
}
